/**
 * @file control_utils
 * @author yq
 * @brief
 * @version 0.1
 * @date 24-3-18
 *
 * @copyright Copyright (c) 2024
 */
#ifndef CONTROLLER_CONTROL_UTILS_H
#define CONTROLLER_CONTROL_UTILS_H

#include "perception_msgs/PerceptionLocalization.h"
#include "perception_msgs/Trajectory.h"
#include "ros/ros.h"

#include <fstream>
#include <cmath>
#include <nav_msgs/Path.h>                  //可视化轨迹
#include <geometry_msgs/PoseStamped.h>      //可视化轨迹
// #include <tf2_ros/transform_broadcaster.h>  //可视化车辆位姿
// #include <geometry_msgs/TransformStamped.h> //可视化车辆位姿
#include <visualization_msgs/Marker.h>      //可视化预测点、预瞄点、位姿
#include <geometry_msgs/Point.h>            //可视化预测点、预瞄点、位姿
#include <geometry_msgs/Pose.h>             //可视化车辆位姿

// 车辆位置信息
struct CarState {
    double x;
    double y;
    double z;
    double yaw; // 横摆角
    double pitch; // 俯仰角
    double vx;  // x方向速度值
    double vy;  // y方向速度值
    double v;   // 合速度
    double acc; // 加速
    double cur; // 曲率
};

struct ControlData  {
    double throttle;// 制动踏板位置范围0~100
    double brake;   // 油门踏板位置范围0~100
    double steer;   // 方向盘
    int gear; // 档位
    ControlData() {
        throttle = 0;
        brake = 0;
        steer = 0;
        gear = 1;
    }
};

struct VehicleConfig {
    double mass; // 质量
    double l; // 轴距
    double steer_ratio; // 转向比
};
double distanceBetweenPoints(double x1, double y1, double x2, double y2);

// 将角度转换为弧度
double degreesToRadians(double degrees);

// 将角度限制在 ±π 范围内
double normalizeAnglePi(double degrees);

double normalizeAngleError(double yaw_error);

// 将角度限制在 0 到 π 范围内
double normalizeAngle0To2Pi(double degrees);

double radiansToDegrees(double radians);

void ExecutiveControl(double acc, ControlData &control_cmd);

// 计算轨迹曲率
void calcTrajectoryCurv(std::shared_ptr<perception_msgs::Trajectory> &ref_trajectory_ptr);

perception_msgs::TrajectoryPoint FindPreviewPoint(const double preview_t,
                                                  const CarState cur_pose,
                                                  const std::shared_ptr<perception_msgs::Trajectory> ref_trajectory_ptr,
                                                  const bool is_print_pre_point_info);
// 输出状态误差
void outErrorToFile(std::ofstream &outFile_error,
                    const std::shared_ptr<perception_msgs::Trajectory> ref_trajectory_ptr,
                    const CarState cur_pose,
                    const long long duration);

void publishTrajectory(const ros::Publisher traj_rviz_pub,
                       const std::shared_ptr<perception_msgs::Trajectory> ref_trajectory_ptr,
                       const perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr);
void publishPreviewPoint(const ros::Publisher predict_point_rviz_pub,
                         const perception_msgs::TrajectoryPoint predict_point,
                         perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr);
void publishPredictPoint(const ros::Publisher predict_point_rviz_pub,
                         const perception_msgs::TrajectoryPoint predict_point,
                         const perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr);
void publishCurPose(const ros::Publisher cur_pose_rviz_pub,
                    const CarState cur_pose,
                    const perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr);

#endif // CONTROLLER_CONTROL_UTILS_H
